10.6 视觉定位

本节内容为理论知识,不是单独的玩法,是课程[7.颜色分拣与堆叠]和课程[8.垃圾分拣]学习前的准备及理论知识。

#运行玩法
dofbot_ws/src/dofbot_color_identify/scripts/颜色分拣玩法.ipynb
#或者
dofbot_ws/src/dofbot_color_stacking/scripts/颜色堆叠玩法.ipynb
#或者
dofbot_ws/src/dofbot_garbage_yolov5/垃圾分拣玩法.ipynb
#可以看到以下示例图片

1.方框标定

标定的目的使物体中心在视野下的位置转换成实际距离机械臂基坐标的位置。

• 示例图片

• 操作流程

(1)启动完所有代码块后，在代码的最下方显示界面，点击【calibration_model】按钮，进入标定模式后，才可以滑动滑动条进行调节。

(2)调节滑动条，使得方框完全出现在视野中，并完整的检测出方框的边缘。滑动条【joint1】控制 1 号舵机左右调整，滑动条【joint2】控制 2 号舵机上下调整，滑动条【threshold】是图像边缘二值化检测阈值的调整。注意 : 必须将方框四边清晰识别出来，且无外界干扰才可以。

(3)点击【calibration_ok】按钮，完成标定。同时会将本次调整的参数写入配置文件中，下次启动时，如无意外就无需调整，直接点击标定和确定即可。

(4)此时，标定结束，如下图所示。如果标定过程中，出现错误。点击【calibration_cancel】按钮取消标定结果，然后重新标定。

• 代码设计

图像处理，提取轮廓点

# 将图像转为灰度图
gray = cv.cvtColor(self.image, cv.COLOR_BGR2GRAY)
# 使用高斯滤镜模糊图像.
gray = cv.GaussianBlur(gray, (5, 5), 1)
# 图像二值化操作
ref, threshold = cv.threshold(gray, self.threshold_num, 255,
cv.THRESH_BINARY)
# 获取不同形状的结构元素
kernel = np.ones((3, 3), np.uint8)
# 形态学开操作
blur = cv.morphologyEx(threshold, cv.MORPH_OPEN, kernel, iterations=4)
# 提取模式
mode = cv.RETR_EXTERNAL
# 提取方法
method = cv.CHAIN_APPROX_NONE
# 获取轮廓点集(坐标) python2 和 python3 在此处略有不同# 层级关系 
参数一：输入的二值图，参数二：提取模式，参数三：提取方法。
contours, hierarchy = cv.findContours(blur, mode, method)

绘制轮廓，返回轮廓的边点

# 遍历点集
for i, c in enumerate(contours):
# 计算轮廓区域.
area = cv.contourArea(c)
# 设置轮廓区域范围
if h * w / 2 < area < h * w:
# 计算多边形的矩
mm = cv.moments(c)
cx = mm['m10'] / mm['m00']
cy = mm['m01'] / mm['m00']
# 绘制轮廓区域
cv.drawContours(self.image, contours, i, (255, 255, 0), 2)
# 获取轮廓区域边点
dp = np.squeeze(cv.approxPolyDP(c, 100, True))
# 绘制中心
cv.circle(self.image, (np.int(cx), np.int(cy)), 5, (0, 0, 255),-1)

透视变换，将检测的轮廓透视变换为(640,480)图像

#将边缘检测的轮廓点分为左上，左下，右下，右上.
for i in range(len(dp)):
if dp[i][0]<320 and dp[i][1]<240: upper_left=dp[i]
if dp[i][0]<320 and dp[i][1]>240: lower_left=dp[i]
if dp[i][0]>320 and dp[i][1]>240: lower_right=dp[i]
if dp[i][0]>320 and dp[i][1]<240: upper_right=dp[i]
# 原图中的四个顶点
pts1 = np.float32([upper_left, lower_left, lower_right, upper_right])
# 变换后的四个顶点
pts2 = np.float32([[0, 0], [0, 480], [640, 480], [640, 0]])
# 根据四对对应点计算透视变换.
M = cv.getPerspectiveTransform(pts1, pts2)
# 将透视变换应用于图像.
Transform_img = cv.warpPerspective(image, M, (640, 480))

详细代码库见

dofbot_ws/src/dofbot_color_identify/scripts/dofbot_config.py

或者

dofbot_ws/src/dofbot_color_stacking/scripts/dofbot_config.py

或者

dofbot_ws/src/dofbot_garbage_yolov5/dofbot_config.py

2.定位计算

• 物体在视野下的定位计算

相机在机械臂上看到地面的视野最广的情况是斜视 45°，此时视野下的方框呈像类似梯形，经透视变换将方框的四边透视为图像的四边。

那么视野下物体中心点距图像(640,480)的中心距离和实际物体中心距方框(160,160)中心的距离就是一个比例关系。

横向计算如下，纵向同理:

(point_x-image_x)/640=(object_x-x)/160

以机械臂基坐标系(URDF 可视化中有介绍)为基准

point_x:视野下物体中心点横向坐标

image_x:图像中心点横向坐标

object_x:实际物体中心点横向坐标

x:基坐标系所在位置，此时为 0

160:方框的边长，单位毫米

在无序分拣中，只有 object_x 是未知项，随机的，其他均已知

• 定位计算示例代码

(a, b) = (round(((point_x - 320) / 4000), 5), round(((480 - point_y) /

3000) * 0.8+0.19, 5))

(a,b):基坐标系下的坐标，是由视野下物体中心点坐标转换而成。

(point_x - 320) / 4000:物体距方框中心点 x 方向的距离。

(480- point_y) / 3000:物体距方框中心点 y 方向的距离。

0.19:基坐标中心点距方框最近边的坐标，手动测量，存在一定的偏差，可适当调

节。

0.8:调节与相机距离不同产生的误差，可适当调节。

((480 - point_y) / 3000) * 0.8+0.19:物体距基坐标中心点 y 方向的距离。

• 逆解角度调节

获取物体在基坐标系下的定位后，逆运动学求出机械臂在规定的姿态各个关节到需要转动的角度。

由于机械臂关节的运动范围为 0°~180°，负的角度值无法到达，当逆解越界，出现负值时，适当调节。

if joints[2] < 0:
    joints[1] += joints[2] * 3 / 5
    joints[3] += joints[2] * 3 / 5
    joints[2] = 0

3. 夹爪计算

由于常根据不同的使用场景使用不同的末端执行器，一般来说，运动学逆解求得是机械臂末端的位姿，并不是末端执行器(夹爪，吸盘等)的末端。

底盘坐标系我们称之为基坐标系，夹爪处的红绿蓝坐标系我们称之为末端坐标系，夹爪称之为末端执行器。现在，运动学逆解求得是末端坐标系(不是夹爪夹取的位置)到达的位置，对于如图所示机械臂，我们在求逆解之前，需要获取的是末端坐标系到达的位置信息(即末端坐标系的位姿信息)，那么如何求解那?

对于姿态，我们设置绕轴顺时针旋转(逆正顺负)如下:

// 根据物体距离不同设置不同的夹取姿态

double Roll = 2.5 * request.tar_y * 100 - 207.5;

double Pitch = 0;

double Yaw = 0;

• 求取偏移角度

xxxxxxxxxx
double init_angle = atan2(double(request.tar_y), double(request.tar_x));

atan2()反正切函数，atan() 的增强版，能确定象限 double

atan2(double y，double x);

atan2() 函数的功能是求 y/x 的反正切值，atan2() 是 atan() 的增强版，能够确定角度所在的象限。反正切函数 atan2() 和正切函数 tan() 的功能恰好相反：

tan()是已知一个角的弧度值，求该角的正切值；而 atan2() 是已知一个角的正切值（也就是 y/x），求该角的弧度值。

• 求夹爪在斜边的投影长度，对应到 xy 平面，如右图所示

测量夹爪的长度，tool_param=0.12;单位是米。

xxxxxxxxxx
double dist = tool_param * sin((180 + Roll) * DE2RA);

• 求机械臂投影到 xy 平面的长度(除去夹爪部分)，对应的距离如右上图，除去红色部分。

xxxxxxxxxx
double distance = hypot(request.tar_x, request.tar_y) - dist;

• 求末端位置(除夹爪)，对应的位置如右上图所示的 x，y 坐标double x = distance * cos(init_angle);

xxxxxxxxxx
double y = distance * sin(init_angle);

• 最后一步求 z 轴坐标

我们要求的是末端坐标系在基坐标系下的 z 坐标，请看左上图三角形，利用三角函数求的竖直直角边高度。

xxxxxxxxxx
double z = tool_param * cos((180 + Roll) * DE2RA);

• 代码实现

// 夹抓长度

double tool_param = 0.12;

// 根据物体距离不同设置不同的夹取姿态

double Roll = 2.5 * request.tar_y * 100 - 207.5;

double Pitch = 0;

double Yaw = 0;

// 求偏移角度

double init_angle = atan2(double(request.tar_y), double(request.tar_x));

// 求夹爪在斜边的投影长度

double dist = tool_param * sin((180 + Roll) * DE2RA);

// 求斜边长度

double distance = hypot(request.tar_x, request.tar_y) - dist;

// 求末端位置(除夹爪)

double x = distance * cos(init_angle);

double y = distance * sin(init_angle);

// 求 z 轴数值

double z = tool_param * cos((180 + Roll) * DE2RA);

详细代码库 dofbot_ws/src/dofbot_info/src/dofbot_server.cpp